#ifndef WHEEL_LEGGED_ROBOT_MOTOR_SOCKETCAN_HPP
#define WHEEL_LEGGED_ROBOT_MOTOR_SOCKETCAN_HPP

#include <chrono>
#include <memory>
#include <string>


#include <iostream>
#include <cstring>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>

namespace my_hardware_interface
{
class wheel_legged_robot_motor_socketcan  // 一条can线上的一个大腿电机和一个轮毂电机共同作为一个对象
{
private:
    int socket_fd_;
    struct sockaddr_can addr_;
    struct ifreq ifr_;

    uint32_t wheel_motor_id_{0x141U};
    uint32_t leg_motor_id_{0x642U};
    double reduction_ratio_{6.0};  // 轮毂电机减速比
    int encorder_counts_{524288}; // 腿关节电机输出端编码器一圈的位数
public:
    wheel_legged_robot_motor_socketcan();
    ~wheel_legged_robot_motor_socketcan();
    // socketcan初始化
    bool socketcan_init(const char* ifname);
    // 轮毂电机启动
    bool wheel_motor_start();
    // 腿关节电机启动
    bool leg_motor_start();
    // 轮毂电机关闭
    bool wheel_motor_shutdown();
    // 腿关节电机关闭
    bool leg_motor_shutdown();
    // 轮毂电机转矩闭环控制命令(输入扭矩单位N*m)
    void wheel_motor_effort_control(double effort);
    // 腿关节电机设置电机扭矩(输入参数为扭矩单位Nm)
    void leg_motor_effort_control(double effort);
    // 轮毂电机读取电机多圈角度绝对值(0.01度/LSB), 返回值为rad
    double wheel_motor_angle_read();
    // 轮毂电机读取电机速度(1dps/LSB), 返回值为rad/s
    double wheel_motor_velocity_read();
    // 腿关节电机读取电机负载端位置(返回的位置单位为rad,为单圈绝对位置)
    double leg_motor_position_read();
    // 轮毂电机读取转矩电流(返回值单位为A)
    double wheel_motor_iq_read();
    // 读取电机实际扭矩(返回扭矩单位为Nm)
    double leg_motor_effort_read();

};

}

#endif